#!/bin/bash

echo "=== 完全修复版点云建图系统 ==="

# 检查ROS 2环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: 未找到ROS 2环境"
    echo "运行: source /opt/ros/humble/setup.bash"
    exit 1
fi

echo "停止可能存在的旧节点..."
ros2 node list | grep -E "(camera|rtabmap)" | while read node; do
    ros2 node kill $node
done
sleep 3

# 获取当前脚本所在目录
SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )

# 1. 启动修复版RealSense
echo "步骤1: 启动修复版RealSense相机..."
gnome-terminal --title="RealSense (Fixed)" -- bash -c "
echo '启动修复版RealSense...';
$SCRIPT_DIR/start_realsense_fixed.sh;
echo 'RealSense节点已停止';
read -p '按Enter键关闭窗口...'
"

echo "等待RealSense初始化..."
# 等待关键话题出现
ros2 topic echo /camera/depth/image_rect_raw --once

# 检查RealSense话题
echo "检查RealSense话题..."
ros2 topic list | grep camera | head -5

# 2. 启动修复版RTAB-Map
echo "步骤2: 启动修复版RTAB-Map..."
gnome-terminal --title="RTAB-Map (Fixed)" -- bash -c "
echo '启动修复版RTAB-Map...';
$SCRIPT_DIR/start_rtabmap_fixed.sh;
echo 'RTAB-Map节点已停止';
read -p '按Enter键关闭窗口...'
"

echo "等待RTAB-Map初始化..."
# 等待RTAB-Map关键话题
ros2 topic echo /rtabmap/info --once

# 3. 启动点云保存服务 (暂时禁用)
# echo "步骤3: 启动点云保存服务..."
# gnome-terminal --title="PointCloud Saver" -- bash -c "
# echo '启动点云保存节点...';
# source /opt/ros/humble/setup.bash;
# python3 $SCRIPT_DIR/save_pointcloud_fixed.py;
# echo '点云保存节点已停止';
# read -p '按Enter键关闭窗口...'
# "

echo ""
echo "=== 系统启动完成 ==="
echo ""
echo "📊 系统状态检查:"
echo "运行 '$SCRIPT_DIR/check_system_status.sh' 检查系统状态"
echo ""
echo "🔧 如果仍有问题，尝试:"
echo "1. 重新插拔RealSense相机USB线"
echo "2. 更换USB 3.0端口"
echo "3. 重启电脑后重试"
